import socket
import threading
import json

from unitree_python_sdk import Unitree_Robot_High
unitree_robot = Unitree_Robot_High()

clients = {}

class client(object):
    def __init__(self, socket, addr, username):
        self.addr = addr[0]
        self.port = addr[1]
        self.username = username
        self.socket = socket
    
    def send(self, msg):
        self.socket.send(msg)
    
    def recv(self, mtu=1024):
        try:
            data = self.socket.recv(mtu)
            if not data:
                return False
            return data
        except:
            return False
    
    def close(self):
        try:
            self.socket.close()
            return True
        except:
            return False

    def id(self):
        return '{0}:{1}'.format(self.addr, self.port)


def new_client(c):
    try:
        while True:
            data = c.recv()
            if not data:
                break
            else:
                data = json.loads(data)
                # print(data)
                unitree_robot.execute(data[0],   # mode
                                      data[1],   # gaitType
                                      data[2],   # speedLevel
                                      data[3],   # footRaiseHeight
                                      data[4],   # bodyHeight
                                      data[5],   # euler
                                      data[6],   # velocity
                                      data[7],   # yawSpeed
                                      data[8])   # reserve

    except socket.error as e:
        print('({0})Socket error: {1}'.format(c.id(), e))
    except Exception as e:
        print('({0})Other exception: {1}'.format(c.id(), e))
    finally:
        print('({0})Client leave.'.format(c.id()))
        c.close()
        clients.pop(c.id())


s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
address = ('192.168.123.161', 8000)
s.bind(address)
s.listen(16)
print('Listening...')

while True:
    conn, addr = s.accept()
    c = client(conn, addr, '')
    clients[c.id()] = c
    t = threading.Thread(target=new_client, args=(c,))
    t.start()
    print('({0})Client entry.'.format(c.id()))
